#include "mfcc.h"
#include "data.h"
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "inputdata.h"
#include "memory_management.h"

//分帧数组，n_frames帧，每帧n_fft点数
float   *frame1 = 0;   //分帧地址
Complex *data1 = 0;//rfft 内存地址 
float   *modV1 = 0;//信号的幅度
float   *powerV1 = 0;//信号的功率谱
float   *powerV1_T = 0;//信号的功率谱 转置 
float   *mel1 = 0;//梅尔频率谱
float   *log_spec1 = 0; //功率频率谱
float   *mfcc1 = 0;// mfcc 谱
float   *mfcc1_T = 0;// mfcc 转置
 
/*
// 打印二维数组的函数，支持不同的格式化字符串
void print_frame(int rows, int cols, void *array, const char *format) {
    int i, j;  // 将循环变量声明移到函数开头
    for (i = 0; i < rows; i++) {
        printf("%d    ", i);
        for (j = 0; j < cols; j++) {
            // 根据传入的格式化字符串打印数组元素
            if (format[0] == '%') {
                if (format[1] == 'd') {
                    //printf(format, ((int (*)[cols])array)[i][j]);
                } else if (format[1] == 'f') {
                    //printf(format, ((float (*)[cols])array)[i][j]);
                }
            }
            //printf(" "); // 每个元素输出后空一格
        }
        //printf("\n\n"); // 每行结束后换行
    }
}*/

/* fmaxf */
float fmaxf(float a, float b) {
    return (a > b) ? a : b;
}


// 交换两个复数
void swap(Complex *a, Complex *b) {
    Complex xdata temp = *a;
    *a = *b;
    *b = temp;
}

// 复数加法
Complex complex_add(Complex a, Complex b) 
{
	Complex xdata result;
	result.real = a.real + b.real;
	result.imag = a.imag + b.imag;
	return result;
}

// 复数减法
Complex complex_sub(Complex a, Complex b) 
{
    Complex xdata result;
    result.real = a.real - b.real;
    result.imag = a.imag - b.imag;
    return result;
}

// 复数乘法
Complex complex_multiply(Complex a, Complex b) {
    Complex xdata result;
    result.real = a.real * b.real - a.imag * b.imag;  // 实部公式
    result.imag = a.real * b.imag + a.imag * b.real;  // 虚部公式
    return result;
}

// 计算 FFT  该函数会计算fft 所有范围， 后续操作只需要取 前/2+1个数据点
void fft(Complex *data1, int n, int inverse) {
    int xdata log_n = 0, i, j, bit1, len;
    float xdata ang;
    Complex xdata wlen, w, u, v;

    while ((1 << log_n) < n) {
        log_n++;
    }

    // 位反转
    for (i = 0; i < n; i++) {
        j = 0;
        for (bit1 = 0; bit1 < log_n; bit1++) {
            if (i & (1 << bit1)) {
                j |= (1 << (log_n - bit1 - 1));
            }
        }
        if (i < j) {
            swap(&data1[i], &data1[j]);
        }
    }

    // Cooley-Tukey 算法
    for (len = 2; len <= n; len <<= 1) {
        ang = 2 * M_PI / len * (inverse ? -1 : 1);
				wlen.real = cos(ang);
				wlen.imag = sin(ang);
        for (i = 0; i < n; i += len) {
						w.real = 1;
						w.imag = 0;
            for (j = 0; j < len / 2; j++) {
                u = data1[i + j];
                v = complex_multiply(data1[i + j + len / 2], w);
                data1[i + j] = complex_add(u, v);
                data1[i + j + len / 2] = complex_sub(u, v);
                w = complex_multiply(w, wlen);
            }
        }
    }

    // 如果是逆变换，需要除以 n
    if (inverse) {
        for (i = 0; i < n; i++) {
            data1[i].real /= n;
            data1[i].imag /= n;
        }
    }
    // 虚数部分符号取反
    for (i = 0; i < n / 2 + 1; i++) {
        data1[i].imag = -data1[i].imag;
    }
}

// rfft 计算
void rfft(int nframes, Complex* data1, int n, int inverse) {
    int xdata i, j;

    for (i = 0; i < nframes * n; i++) {
        data1[i].real = *(frame1 + i); // 使用 frame1 的数据
        data1[i].imag = 0.0;
    }

    for (j = 0; j < nframes; j++) {
        fft(data1 + j * n, n, inverse);
    }

    // 打印结果
    //for (j = 0; j < 32; j++) {
    //    for (i = 0; i < n; i++) {
            // printf("n_frames=%d,data[%d] = (%f, %f)\n", j, i, data[j * n + i].real, data[j * n + i].imag);
    //    }
    //}
}
 
// 矩阵转置函数
void transpose(float *in, float *out, int in_rows, int in_cols) {
    int xdata i, j, in_index, out_index;  // 将循环变量声明移到函数开头
    for (i = 0; i < in_rows; i++) {
        for (j = 0; j < in_cols; j++) {
            // 计算输入矩阵中元素的索引
            in_index = i * in_cols + j;
            // 计算输出矩阵中元素的索引
            out_index = j * in_rows + i;
            // 将输入矩阵的元素复制到输出矩阵的正确位置
            out[out_index] = in[in_index];
        }
    }
}

// 矩阵乘法运算
void matrixMultiply(float a[], float b[], float c[], int rowsA, int colsA, int rowsB, int colsB) {
    int xdata i, j, k;  // 将循环变量声明移到函数开头
		rowsB = 0;
    for (i = 0; i < rowsA; i++) {
        for (j = 0; j < colsB; j++) {
            c[i * colsB + j] = 0; // 初始化结果矩阵C的元素为0
            for (k = 0; k < colsA; k++) {
                c[i * colsB + j] += a[i * colsA + k] * b[k * colsB + j];
            }
        }
    }
}


// 将功率谱转换为分贝值
void power_to_db(float *S, int size, float ref, float amin, float top_db, float *log_spec) {
    int xdata i;  // 将循环变量声明移到函数开头
    float xdata ref_value = fabs(ref);
    float xdata max_log_spec = -FLT_MAX;

    // 计算对数功率谱
    for (i = 0; i < size; i++) {
        log_spec[i] = 10.0 * log10(fmaxf(amin, S[i]));
        log_spec[i] -= 10.0 * log10(fmaxf(amin, ref_value));
        if (log_spec[i] > max_log_spec) {
            max_log_spec = log_spec[i];
        }
    }

    // 限制分贝值的范围
    if (top_db >= 0) {
        float min_log_spec = max_log_spec - top_db;
        for (i = 0; i < size; i++) {
            log_spec[i] = fmaxf(log_spec[i], min_log_spec);
        }
    } else {
        //fprintf(stderr, "top_db must be non-negative\n");
        return;
    }
}

// 函数：将矩阵的每个元素乘以一个常数
void multiplyMatrixByNumber(float *matrix, int number, int rows, int cols) {
    int xdata i, j, index;  // 将循环变量声明移到函数开头
    for (i = 0; i < rows; i++) {
        for (j = 0; j < cols; j++) {
            // 计算一维数组中的索引
            index = i * cols + j;
            // 将元素乘以 number
            matrix[index] *= number;
        }
    }
}

// 正交归一化函数
void ortho_normalize(float *y, int rows, int cols) {
    int xdata i, j, index;  // 将循环变量声明移到函数开头
    float xdata scale[32];  // 用于存储缩放因子
    // 计算缩放因子
    scale[0] = 1.0 / sqrt(4 * cols);
    for (i = 1; i < cols; i++) {
        scale[i] = 1.0 / sqrt(2 * cols);
    }

    // 应用缩放因子
    for (i = 0; i < rows; i++) {
        for (j = 0; j < cols; j++) {
            index = i * cols + j;
            y[index] *= scale[j];
        }
    }
}
void frame_to_mfcc(void) 
{
    int xdata i, j, start, end, temp;  // 将循环变量和临时变量声明移到函数开头
    //printf("in_pek addr = %p\n", (void*)in_pek);  
    // 执行后的内存情况：
    // in      0      到  8000 -1  共 8000字节，
    // in_pek  8000   到  24000-1  共 16000字节，
    // 后面空白 
    // 波形居中， 波形 前后增加 16个0 
    in_pek = (float *)(rampool + 8000 - 16*4);
    for (i = 0; i < 16; i++) 
    {
      *in_pek = 0;
      in_pek ++;
    }
    in_pek = (float *)(rampool + 24000 + 16*4);
    for (i = 0; i < 16; i++) 
    {
      *(in_pek) = 0;
      in_pek ++;
    }
    in_pek = (float *)(rampool + 7936);
   // print_frame(1, 4000, in_pek, "%f"); 
    // 执行后的内存情况：
    // in      0      到  8000 -1  共 8000字节，
    // in_pek  7936   到  24064-1  共 16128字节，
    // 后面空白 
    // 分帧
    // 1-分帧的数据存储在内存池偏移 8064 个字节地址，注意尽量地址是4字节对齐
    // frame1 大小为 4 * N_FRAMES * N_FFT = 4096字节
    frame1 = (float *)(rampool + 24064);
    // 执行后的内存情况：
    // in      0      到  8000 -1  共 8000字节，
    // in_pek  7936   到  24064-1  共 16000字节，
    // frame1  24064  到  28160-1  共 4096字节
    // 后面空白 
    for (i = 0; i < N_FRAMES; i++) 
    {
        // 计算当前帧的起始位置
        start = i * HOP_LENGTH;
        // 计算当前帧的结束位置
        end = start + N_FFT;
        // 保存分帧 
        for (j = start; j < end; j++) 
        {
           //printf("%d,%d,%f\n", i * N_FRAMES + j - start,    7936 + j * 4,*((float *)(rampool+ 7936)+j));
            *(frame1 + i * N_FRAMES + j - start) = *((float *)(rampool+ 7936)+j);
        }
    }
    //print_frame(32,32,frame1,"%f");
    // 每帧点乘加窗
    for (i = 0; i < N_FRAMES; i++) 
    {
        // 保存分帧 
        for (j = 0; j < N_FFT; j++) 
        {
            *(frame1 + i * N_FRAMES + j) = *(frame1 + i * N_FRAMES + j) * window[j]; 
        }
    }
		
    
    compact_memory(rampool, (void**)&frame1, 4096); 
    //printf("frame1 addr = %p\n", (void*)frame1);
		
    // 执行后的内存情况：frame1 0 到 4095 共 4096字节，data1 地址从 4096 到 12287 共 8192 字节，后面空白
    data1 = (Complex *)(rampool + 4096);
    //printf("data1 addr = %p\n", (void*)data1);
		
    // 加窗后对每帧进行 FFT 计算
    rfft(N_FRAMES, data1, N_FFT, 0);

    // 计算信号的幅度：实部平方 + 虚部平方后开根号
    modV1 = (float *)(rampool + 12288);
    //printf("modV1 addr = %p\n", (void*)modV1);

    for (i = 0; i < N_FRAMES; i++) 
    {
        for (j = 0; j < N_RFFT; j++) 
        {
            temp = i * N_RFFT + j;
            modV1[temp] = sqrt(data1[i * N_FRAMES + j].real * data1[i * N_FRAMES + j].real + 
                               data1[i * N_FRAMES + j].imag * data1[i * N_FRAMES + j].imag);
        }
    }

    // 计算功率谱
    powerV1 = (float *)(rampool + 14464);
    //printf("powerV1 addr = %p\n", (void*)powerV1);

    for (i = 0; i < N_FRAMES * N_RFFT; i++) 
    {
        powerV1[i] = pow(modV1[i], POWERT);
    }

    // 计算梅尔频率谱 = 功率谱与梅尔频率谱点乘
    powerV1_T = (float *)(rampool + 16640);
    //printf("powerV1_T addr = %p\n", (void*)powerV1_T);

    // 先矩阵转置
    transpose((float *)powerV1, (float *)powerV1_T, N_FRAMES, N_RFFT);

    mel1 = (float *)(rampool + 18816);
    //printf("mel1 addr = %p\n", (void*)mel1);

    // 矩阵乘法
    matrixMultiply((float *)melbasis, (float *)powerV1_T, (float *)mel1, N_FRAMES, N_RFFT, N_RFFT, N_FRAMES);

    // 将梅尔频谱图转为 dB 功率谱图
    log_spec1 = (float *)(rampool + 22912);
    //printf("log_spec1 addr = %p\n", (void*)log_spec1);

    power_to_db((float *)mel1, N_FRAMES * N_FRAMES, 1.0, 1e-10, 80.0, (float *)log_spec1);

    // 内存整理，将有用的数据往前移动
    compact_memory(rampool, (void**)&log_spec1, 4096); 
    //printf("log_spec1 addr = %p\n", (void*)log_spec1);

    // dB 功率谱矩阵乘离散余弦矩阵得到 MFCC 矩阵
    mfcc1 = (float *)(rampool + 4096);
    //printf("mfcc1 addr = %p\n", (void*)mfcc1);

    matrixMultiply((float*)cos_matrix, (float*)log_spec1, (float*)mfcc1, 32, 32, 32, 32); 
    multiplyMatrixByNumber((float*)mfcc1, 2, 32, 32);

    mfcc1_T = (float *)(rampool + 8192);
    //printf("mfcc1_T addr = %p\n", (void*)mfcc1_T);

    transpose((float *)mfcc1, (float *)mfcc1_T, 32, 32);

    // 输出归一化
    ortho_normalize((float*)mfcc1_T, 32, 32);
    transpose((float *)mfcc1_T, (float *)mfcc1, 32, 32);

    // 内存整理，将有用的数据往前移动
    compact_memory(rampool, (void**)&mfcc1, 4096); 
    //printf("mfcc1 addr = %p\n", (void*)mfcc1);
		/**/
}